package legway;

import java.io.File;
import lejos.robotics.subsumption.*;
import lejos.nxt.*;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;

/**
 * A behavior which allows the Legway to receive Bluetooth commands to turn them into 
 * actions in the robot. Note that this behavior requires an active Bluetooth connection.
 * 
 * @author Kim Petersen, Andreas Koefoed-Hansen og Tim Rasmussen
 * @version Januar 2010
 */
public class BTDriver implements Behavior
{
	// Key codes
	private static final int STOP = 0x0;
	private static final int FORWARD = 0x1;
	private static final int BACKWARD = 0x2;
	private static final int LEFT = 0x4;
	private static final int RIGHT = 0x8;
	private static final int FORWARD_LEFT = FORWARD | LEFT;
	private static final int FORWARD_RIGHT = FORWARD | RIGHT;
	private static final int BACKWARD_LEFT =  BACKWARD | LEFT;
	private static final int BACKWARD_RIGHT =  BACKWARD | RIGHT;
	private static final int DRIVECOMMANDS = FORWARD | BACKWARD | LEFT | RIGHT;
	private static final int HORN = 0x10;
	private static final int AVOID = 0x20;
	private static final int FOLLOW = 0x40;

	private int prevoiusCommand;
	private boolean isConnected = false;
	private BTConnection btc;
	private BTCommandReader br;
	private Car car;


	/**
	 * BTDriver constructor.
	 * 
	 * @param ctrl The motor control parameters.
	 */
	public BTDriver(CtrlParam ctrl)	{
		car = new Car(ctrl);
		ConnectionListener cl = new ConnectionListener();
		cl.start();
	}

	/**
	 * A thread listening for connections
	 */
	private class ConnectionListener extends Thread {

		public ConnectionListener() {
			this.setDaemon(true);
		}

		public void run() {
			while (true) {
				//LCD.drawString("Waiting...", 2, 1);
				btc = Bluetooth.waitForConnection();
				if(btc == null)
					continue;
				//LCD.drawString("Connected ", 2, 1);
				br = new BTCommandReader(btc, this);
				br.start();
				isConnected = true;
				Sound.beepSequenceUp();
				// Sleep until connection is lost
				try {
					Thread.sleep(Integer.MAX_VALUE);
				} catch (InterruptedException e) {
					//Stop legway
					car.stop();
					isConnected = false;
					Sound.beepSequence();
				}
			}
		}
	}

	/**
	 * Take control iff there is an active Bluetooth connection
	 */
	public boolean takeControl() {
		return isConnected;
	}

	/**
	 * No suppression required
	 */
	public void suppress() {

	}

	/**
	 * Action method which converts a sent command into a movement action in the robot.
	 */
	public void action() {
		try{
			int command = br.getCommand();
			if(command != prevoiusCommand) {
				switch (command & DRIVECOMMANDS)
				{
				case LEFT:
					//LCD.drawString("LEFT", 5, 5);
					car.left();
					break;
				case RIGHT:
					//LCD.drawString("RIGHT", 5, 5);
					car.right();
					break;
				case FORWARD:
					//LCD.drawString("FORWARD", 3, 5);
					car.forward();
					break;
				case FORWARD_LEFT:
					//LCD.drawString("FORWARD_LEFT", 3, 5);
					car.forwardLeft();
					break;
				case FORWARD_RIGHT:
					//LCD.drawString("FORWARD_RIGHT", 3, 5);
					car.forwardRight();
					break;
				case BACKWARD:
					//LCD.drawString("BACKWARD", 3, 5);
					car.backward();
					break;
				case BACKWARD_LEFT:
					//LCD.drawString("BACKWARD_LEFT", 3, 5);
					car.backwardLeft();
					break;
				case BACKWARD_RIGHT:
					//LCD.drawString("BACKWARD_RIGHT", 3, 5);
					car.backwardRight();
					break;
				case STOP:
					//LCD.drawString("STOP", 3, 5);
					car.stop();
					break;
				}
				if((command & HORN) != 0) {
					File hornFile = new File("horn.wav");
					if(hornFile.canRead())
						Sound.playSample(hornFile);
				}
				if((command & AVOID) != 0) {
					AvoidFront.setEnabled(true);
				}
				else {
					AvoidFront.setEnabled(false);
				}
				if((command & FOLLOW) != 0) {
					FollowLine.setEnabled(true);
				}
				else {
					FollowLine.setEnabled(false);
				}
				prevoiusCommand = command;
			}
		}
		catch (NullPointerException e) // Connection can be lost and GC'ed
		{}
	}
}
